/********************************************************************
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* Thank friend GG_ruler for his help 
* @brief		    
This file is used to write remote control programs
* @author           Chuyan

********************************************************************/

#include "bluetooth.h"

BLUETOOTH RC;
SbusStruct SBUS;
// Sbus_get(RC_UART, &SBUS);
//-------------------------------------------------------------------------------------------------------------------
// @brief		    
// @param		    
// @return		    
// @since		    v1.0
// Sample usage:	Key_Judge();			
//-------------------------------------------------------------------------------------------------------------------
void Sbus_get(uart_index_enum uartn, SbusStruct* sbusData)
{
    uart_query(uartn, &sbusData->rx[sbusData->i++]);
    if (sbusData->rx[0] != 0x0f)  // Determine the frame header
    {
        sbusData->i = 0;
    }
    if(sbusData->i >= 24)
    {
        if (sbusData->rx[24] == 0x00)
        {
            Sbus_Data_Count(sbusData);
            sbusData->i = 0;
        }
    }
    
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    
// @param		    
// @return		    
// Sample usage:	Sbus_Data_Count(sbusData);		
//-------------------------------------------------------------------------------------------------------------------
void Sbus_Data_Count(SbusStruct* sbusData)      
{
    
	sbusData->CH[0] = (sbusData->rx[ 1] >> 0 | ((int16)sbusData->rx[ 2] << 8 )) & 0x07FF;
	sbusData->CH[1] = (sbusData->rx[ 2] >> 3 | ((int16)sbusData->rx[ 3] << 5 )) & 0x07FF;
	sbusData->CH[2] = (sbusData->rx[ 3] >> 6 | ((int16)sbusData->rx[ 4] << 2 ) | (int16)sbusData->rx[ 5] << 10 ) & 0x07FF;
	sbusData->CH[3] = (sbusData->rx[ 5] >> 1 | ((int16)sbusData->rx[ 6] << 7 )) & 0x07FF;
	sbusData->CH[4] = (sbusData->rx[ 6] >> 4 | ((int16)sbusData->rx[ 7] << 4 )) & 0x07FF;
    sbus();
    
    // Channels that are not used
    // sbusData->CH[5] = (sbusData->rx[ 7] >> 7 | ((int16)sbusData->rx[ 8] << 1 ) | (int16)sbusData->rx[9] << 9 ) & 0x07FF;
    // sbusData->CH[6] = (sbusData->rx[ 9] >> 2 | ((int16)sbusData->rx[10] << 6 )) & 0x07FF; 
	//  sbusData->CH[ 7] = ((int16)sbusData->rx[10] >> 5 | ((int16)sbusData->rx[11] << 3 )) & 0x07FF;
	//  sbusData->CH[ 8] = ((int16)sbusData->rx[12] << 0 | ((int16)sbusData->rx[13] << 8 )) & 0x07FF;
	//  sbusData->CH[ 9] = ((int16)sbusData->rx[13] >> 3 | ((int16)sbusData->rx[14] << 5 )) & 0x07FF;
	//  sbusData->CH[10] = ((int16)sbusData->rx[14] >> 6 | ((int16)sbusData->rx[15] << 2 ) | (int16)sbusData->rx[16] << 10 ) & 0x07FF;
	//  sbusData->CH[11] = ((int16)sbusData->rx[16] >> 1 | ((int16)sbusData->rx[17] << 7 )) & 0x07FF;
	//  sbusData->CH[12] = ((int16)sbusData->rx[17] >> 4 | ((int16)sbusData->rx[18] << 4 )) & 0x07FF;
	//  sbusData->CH[13] = ((int16)sbusData->rx[18] >> 7 | ((int16)sbusData->rx[19] << 1 ) | (int16)sbusData->rx[20] << 9 ) & 0x07FF;
	//  sbusData->CH[14] = ((int16)sbusData->rx[20] >> 2 | ((int16)sbusData->rx[21] << 6 )) & 0x07FF;
	//  sbusData->CH[15] = ((int16)sbusData->rx[21] >> 5 | ((int16)sbusData->rx[22] << 3 )) & 0x07FF;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief		    Parse the SBUS signal
// @param		    
// @return		    
// Sample usage:	sbus();			
//-------------------------------------------------------------------------------------------------------------------
void sbus()
{
    if(SBUS.CH[4] > 1790)          RC.STATE = RC_stop; //  Stop mode
    else  if(SBUS.CH[4] < 210)     
    {
        RC.STATE = RC_remote;// Remote control mode
        
        V_duty =(float)(SBUS.CH[1]-1088)/(1492-1088)*100*0.60;
        
        Servo_PWM = SERVO_MEDIAN + (SBUS.CH[0] - 916)/2;
        
        if(V_duty<5 && V_duty>-5)
        {
            V_duty = 0;
        }
        else if(V_duty<=-5)
        {
            V_duty=V_duty*0.5f;
        }
        // Servo_PWM = Loca_PID(&Servo_gyro_pid, time_k, WIT.WITangle_gyro_z);
    }
    
    
}

